#include "precomp.h"
#include "GoalFollowPath.h"
#include "GoalMoveToPos.h"
#include "../Entity.h"
#include "../../Map/Grid.h"

namespace Tmpl {

GoalFollowPath::GoalFollowPath(Entity* owner) : Goal(owner)
{
	mMaxPathLength = 8;
	mPath = MALLOC64(mMaxPathLength, Vec2f);
	mPathLength = 0;

	mCurrentSegment = 0;
	mLoop = false;
}
GoalFollowPath::~GoalFollowPath()
{
	if (mPath) FREE64(mPath);
	mPath = 0;
}

void GoalFollowPath::AddPathPoint(Vec2f pos)
{
	if (mPathLength == mMaxPathLength)
	{
		mMaxPathLength *= 4;
		mPath = REALLOC64(mPath, mMaxPathLength, Vec2f);
	}
	mPath[mPathLength++] = pos;
}
void GoalFollowPath::SetPathPoint(Vec2f pos, int idx)
{
	while(idx >= mMaxPathLength)
	{
		mMaxPathLength *= 2;
		mPath = REALLOC64(mPath, mMaxPathLength, Vec2f);
	}
	mPath[idx] = pos;
	if (idx >= mPathLength) mPathLength = idx + 1;
}

void GoalFollowPath::Activate()
{
	mStatus = Goal::GOALSTATUS_ACTIVE;
}

void GoalFollowPath::Process(char threadIds)
{
	if (mCurrentSegment >= mPathLength)
	{
		mStatus = GOALSTATUS_COMPLETED;
	}
	else
	{
		// get data
		float maxSpeed = mOwner->GetMaxSpeed();
		Vec2f vDist = mPath[mCurrentSegment] - mOwner->GetPos();
		float len = vDist.Length();

		// update entity
		if ( len > 0.0f ) mOwner->SetFacing(vDist / len);
		mOwner->SetSpeed(len);

		// distance from point untill it is finished
		float dist;
		if (mCurrentSegment == mPathLength-1 && !mLoop)
		{
			dist = mOwner->GetSize();
		}
		else
		{
			dist = mOwner->GetSize() + 0.5f;
		}

		// Update Status
		if (len < maxSpeed + dist)
		{
			if (++mCurrentSegment == mPathLength)
			{
				if (mLoop) 
				{
					mCurrentSegment = 0;
				}
				else
				{
					mStatus = GOALSTATUS_COMPLETED;
					return;
				}

				// Calculate the time it should take
			}
		}
	}
}
void GoalFollowPath::Terminate()
{
	mOwner->SetSpeed(0.0f);
}
void GoalFollowPath::Render() const
{
	Vec2f start, end;

	// Render Path
	if (mLoop)
	{
		for (int i=0; i<mPathLength-1; ++i)
		{
			Render::RenderLine2D(mPath[i], mPath[i+1], Vec3f(0.7f,0.5f,0));
		}
		Render::RenderLine2D(mPath[mPathLength-1], mPath[0], Vec3f(0.7f,0.5f,0));
	}
	else
	{
		for (int i=mCurrentSegment; i<mPathLength-1; ++i)
		{
			Render::RenderLine2D(mPath[i], mPath[i+1], Vec3f(0.7f,0.5f,0));
		}
	}

	// Render current
	if (mStatus == GOALSTATUS_ACTIVE)
	{
		Render::RenderLine2D(mOwner->GetPos(), mPath[mCurrentSegment], Vec3f(1,1,0));
	}
}


}
